// old code takern out of user control code:


void ScoreYellowProg(bool secondTry)
{
	// TODO - consider making the arm go down here just in case it gets interrupted in main autonomous loop
	//so far not a problem
	// flipper down to pick up five stack (opens jaws)
	if(secondTry == true)
	{
		while (SensorValue [FlipperPot] > 3040)
		{
			motor(FlipperMotorLeft) = -63;
			motor(FlipperMotorRight) = -63;
		}
		motor(FlipperMotorLeft) = 0;
		motor(FlipperMotorRight) = 0;
	}
	else
	{
		flippergodown();
	}

	//drive forward to yellow bag
	MoveFullSpeed(true, 1500);

	// raise trough slightly and hold
	motor[FlipperMotorLeft] = 127;
	motor[FlipperMotorRight] = 127;
	MoveFullSpeed(true, 600);
	//wait1Msec(200);
	// TODO - consider not having the flipper motor on - it always goes back against arm and we usually lose a bag doing that
	motor[FlipperMotorLeft] = 10;
	motor[FlipperMotorRight] = 10;

	// back up
	MoveFullSpeed(false, 1250);
	// lowering flipper so bags dont fall out
	motor[FlipperMotorLeft] = -80;
	motor[FlipperMotorRight] = -80;
	wait1Msec(250);
	motor[FlipperMotorLeft] = 10;
	motor[FlipperMotorRight] = 10;

	// raise arms up to trough
	Arms(true, ArmsTrough, 127);

	// drive forward
	if(secondTry == false)
	{
		MoveFullSpeed(true, 250);
	}
	else
	{
		MoveFullSpeed(true, 210);
	}

	// score five stack
	flippertotrough();
	//wait1Msec(500);

	// drive back to starting tile
	MoveFullSpeed(false, 1200);
}


void ScoreTrough()
{
	bool go = true;
	//part 1 drive forward
	while(go == true)
	{
		motor[RightBaseFront] = 127;
		motor[RightBaseBack] = 127;
		motor[LeftBaseFront] = 127;
		motor[LeftBaseBack] = 127;
		wait1Msec(600);
		go = false;
	}
	motor[RightBaseFront] = 0;
	motor[RightBaseBack] = 0;
	motor[LeftBaseFront] = 0;
	motor[LeftBaseBack] = 0;
	//part 2 turn 90 degrees
	//While the absolute value of the gyro is less than the desired rotation...
	while(SensorValue[Gyro] > -800)
	{
		//...continue turning
		motor[RightBaseFront] = -127;
		motor[RightBaseBack] = -127;
		motor[LeftBaseFront] = 127;
		motor[LeftBaseFront]  = 127;
	}
	motor[RightBaseFront] = 0;
	motor[RightBaseBack] = 0;
	motor[LeftBaseFront] = 0;
	motor[LeftBaseBack] = 0;

	//part 3 raise arms to trough
	while(SensorValue(ArmPot) < Trough)
	{
		motor(RightArm) = 127;
		motor(LeftArm) = 127;
	}
	motor(RightArm) = 5;
	motor(LeftArm) = 5;
	go = true;
	//part 1 drive forward
	while(go == true)
	{
		motor[RightBaseFront] = 127;
		motor[RightBaseBack] = 127;
		motor[LeftBaseFront] = 127;
		motor[LeftBaseBack] = 127;
		wait1Msec(1000);
		go = false;
	}
	motor[RightBaseFront] = 0;
	motor[RightBaseBack] = 0;
	motor[LeftBaseFront] = 0;
	motor[LeftBaseBack] = 0;
	flippergodown();
	go = true;
	//part drive back
	while(go == true)
	{
		motor[RightBaseFront] = -127;
		motor[RightBaseBack] = -127;
		motor[LeftBaseFront] = -127;
		motor[LeftBaseBack] = -127;
		wait1Msec(1000);
		go = false;
	}
	motor[RightBaseFront] = 0;
	motor[RightBaseBack] = 0;
	motor[LeftBaseFront] = 0;
	motor[LeftBaseBack] = 0;

	flipperup();
	//part  raise arms to floor
	while(SensorValue(ArmPot) > LowLimit)
	{
		motor(RightArm) = -63;
		motor(LeftArm) = -63;
	}
	motor(RightArm) = 5;
	motor(LeftArm) = 5;
}


void ScoreFive()
{
	bool go = true;
	//part 1 drive forward
	while(go == true)
	{
		motor[RightBaseFront] = 127;
		motor[RightBaseBack] = 127;
		motor[LeftBaseFront] = 127;
		motor[LeftBaseBack] = 127;
		wait1Msec(700);
		go = false;
	}
	motor[RightBaseFront] = 0;
	motor[RightBaseBack] = 0;
	motor[LeftBaseFront] = 0;
	motor[LeftBaseBack] = 0;
	//part 2 turn 90 degrees
	//While the absolute value of the gyro is less than the desired rotation...
	while(SensorValue[Gyro] > -650)
	{
		//...continue turning
		motor[RightBaseFront] = -127;
		motor[RightBaseBack] = -127;
		motor[LeftBaseFront] = 127;
		motor[LeftBaseFront]  = 127;
	}
	motor[RightBaseFront] = 0;
	motor[RightBaseBack] = 0;
	motor[LeftBaseFront] = 0;
	motor[LeftBaseBack] = 0;

	flippergodown();
	//part 1 drive forward
	go = true;
	while(go == true)
	{
		motor[RightBaseFront] = 127;
		motor[RightBaseBack] = 127;
		motor[LeftBaseFront] = 127;
		motor[LeftBaseBack] = 127;
		wait1Msec(1000);
		go = false;
	}
	motor[RightBaseFront] = 0;
	motor[RightBaseBack] = 0;
	motor[LeftBaseFront] = 0;
	motor[LeftBaseBack] = 0;
	flipperup();
	//part 3 raise arms to trough
	while(SensorValue(ArmPot) < Trough)
	{
		motor(RightArm) = 127;
		motor(LeftArm) = 127;
	}
	motor(RightArm) = 10;
	motor(LeftArm) = 10;
	go = true;
	//part 1 drive forward
	while(go == true)
	{
		motor[RightBaseFront] = 127;
		motor[RightBaseBack] = 127;
		motor[LeftBaseFront] = 127;
		motor[LeftBaseBack] = 127;
		wait1Msec(200);
		go = false;
	}
	motor[RightBaseFront] = 0;
	motor[RightBaseBack] = 0;
	motor[LeftBaseFront] = 0;
	motor[LeftBaseBack] = 0;

	flippertotrough();
	wait1Msec(500);

	wait1Msec(300);
	go = true;
	//part drive back
	while(go == true)
	{
		motor[RightBaseFront] = -127;
		motor[RightBaseBack] = -127;
		motor[LeftBaseFront] = -127;
		motor[LeftBaseBack] = -127;
		wait1Msec(1000);
		go = false;
	}
	motor[RightBaseFront] = 0;
	motor[RightBaseBack] = 0;
	motor[LeftBaseFront] = 0;
	motor[LeftBaseBack] = 0;
}



void Drive()
{
	bool go = true;
	//part 1 drive forward
	while(go == true)
	{
		motor[RightBaseFront] = 127;
		motor[RightBaseBack] = 127;
		motor[LeftBaseFront] = 127;
		motor[LeftBaseBack] = 127;
		wait1Msec(1000);
		go = false;
	}
	go = true;
	//part 2 drive backward
	while(go == true)
	{
		motor[RightBaseFront] = -127;
		motor[RightBaseBack] = -127;
		motor[LeftBaseFront] = -127;
		motor[LeftBaseBack] = -127;
		wait1Msec(1000);
		go = false;
	}
	motor[RightBaseFront] = 0;
	motor[RightBaseBack] = 0;
	motor[LeftBaseFront] = 0;
	motor[LeftBaseBack] = 0;
}



void flippertotroughprog()
{
	// first reead the pot on flipper into a local variable
	// keep movimg the flliper down until it reaCHES  the "trough" pot setting
	// once it reaches the trough setting - quit moving the flipper (open jaws?)
	while (SensorValue[FlipperPot] > FlipperScore)
	{
		motor(FlipperMotorRight) = -127;
		motor(FlipperMotorLeft) = -127;
	}

	motor(FlipperMotorLeft) = 0;
	motor(FlipperMotorRight) = 0;
}



void flippergodownprog()
{
	while (SensorValue [FlipperPot] > FlipperToFloor)
	{
		motor(FlipperMotorLeft) = -63;
		motor(FlipperMotorRight) = -63;
		//motor(TheJaw) = 127;
	}

	motor(FlipperMotorLeft) = 0;
	motor(FlipperMotorRight) = 0;
	//motor(TheJaw) = 10;
}



void flippertotrough()
{
	while (SensorValue[FlipperPot] > FlipperScore)
	{
		motor(FlipperMotorRight) = -127;
		motor(FlipperMotorLeft) = -127;
	}

	motor(FlipperMotorLeft) = 0;
	motor(FlipperMotorRight) = 0;
}



void flippergodown()
{
	motor(FlipperMotorLeft) = -63;
	motor(FlipperMotorRight) = -63;
	wait1Msec(1000);
	motor(FlipperMotorLeft) = 0;
	motor(FlipperMotorRight) = 0;
}




void flipperup1()
{
	while (SensorValue [FlipperPot] < 3641)
	{
		motor(FlipperMotorLeft) = 127;
		motor(FlipperMotorRight) = 127;
		if (SensorValue [FlipperPot] > 3641)
		{
			break;
		}
	}
	wait1Msec(1000);
	motor(FlipperMotorLeft) = 10;
	motor(FlipperMotorRight) = 10;

}


void flipperup()
{
	motor(FlipperMotorLeft) = 127;
	motor(FlipperMotorRight) = 127;
	wait1Msec(2000);
	motor(FlipperMotorLeft) = 10;
	motor(FlipperMotorRight) = 10;
}



#define FlipperScoreHeavy 	-100
#define FlipperUp 					100
#define FlipperHoldHeavy 		25
#define FlipperHoldLight 		10
#define FlipperFast 				127
#define FlipperSlow 				60
#define FlipperMedium 			90
